#include <iostream>
#include <math.h>
#include <fstream>
#define pi 3.1415926
using namespace std;
class leader
{
public:
    int iteration, round;
    double *x, *y;
    double (*func)(double x);
    leader(int n, double (*px)(double x));
    double *iterative(int round);
};
class follower
{
public:
    int id;
    int iteration, round;
    double *x, *y, dis, error[2], integrator[2], P, I, D, dt;
    follower(int n, double d, int num, double PID[3], double t);
    void iterative(int round, double *p);
};
leader::leader(int n, double (*px)(double x))
{
    iteration = n;
    x = new double[n + 1];
    y = new double[n + 1];
    func = px;
    round = 1;
};
double *leader::iterative(int round)
{
    double *p = new double[3];
    double ang;
    if (round < 50)
    {
        x[round] = 1.0 * round / 20;
        y[round] = 1;
    }
    else
    {
        x[round] = 1.0 * round / 20;
        y[round] = (*func)(x[round]);
    }
    if (round >= 2)
    {
        ang = atan(1.0 * (y[round] - y[round - 1]) / (x[round] - x[round - 1]));
    }
    else
    {
        ang = 0;
    }
    p[0] = x[round];
    p[1] = y[round];
    p[2] = ang;
    return p;
};
follower::follower(int n, double d, int num, double PID[3], double t)
{
    iteration = n;
    dis = d;
    id = num;
    dt = t;
    error[0] = error[1] = integrator[0] = integrator[1] = 0;
    P = PID[0];
    I = PID[1];
    D = PID[2];
    x = new double[n + 1];
    y = new double[n + 1];
};
void follower::iterative(int round, double *p)
{
    double target_x, target_y, error_x, error_y, P_x, P_y, I_x, I_y, D_x, D_y, out_x, out_y;
    if (round == 1)
    {
        if (id == 1)
        {
            x[round] = -0.5;
            y[round] = 1.5;
        }
        else if (id == 2)
        {
            x[round] = -0.5;
            y[round] = 0.5;
        }
    }
    if (id == 1)
    {
        target_x = p[0] - dis * cos(p[2]) - dis * sin(p[2]);
        target_y = p[1] - dis * sin(p[2]) + dis * cos(p[2]);
    }
    else if (id == 2)
    {
        target_x = p[0] - dis * cos(p[2]) + dis * sin(p[2]);
        target_y = p[1] - dis * sin(p[2]) - dis * cos(p[2]);
    }
    error_x = target_x - x[round - 1];
    error_y = target_y - y[round - 1];
    integrator[0] += error_x;
    integrator[1] += error_y;
    P_x = error_x;
    P_y = error_y;
    I_x = integrator[0] * dt;
    I_y = integrator[1] * dt;
    D_x = (error_x - error[0]) / dt;
    D_y = (error_y - error[1]) / dt;
    error[0] = error_x;
    error[1] = error_y;
    out_x = P * P_x + I * I_x + D * D_x;
    out_y = P * P_y + I * I_y + D * D_y;
    if (round >= 2)
    {
        x[round] = x[round - 1] + out_x;
        y[round] = y[round - 1] + out_y;
    }
}
double sinx(double x)
{
    return sin(x - 2.5 + pi / 2);
};

int main()
{
    int iteration = 250;
    double (*func)(double x) = sinx;
    double dis = 0.4;
    double PID[3] = {0.6, 0.01, 0.001};
    double *p = new double[3];
    double dt = 0.005;
    ofstream oFile;
    leader car_leader(iteration, func);
    follower car_follower_1(iteration, dis, 1, PID, dt), car_follower_2(iteration, dis, 2, PID, dt);
    oFile.open("pid_data.csv", ios::out | ios::trunc);

    for (int i = 1; i <= iteration; i++)
    {
        p = car_leader.iterative(i);
        car_follower_1.iterative(i, p);
        car_follower_2.iterative(i, p);

        oFile << car_leader.x[i] << "," << car_leader.y[i] << "," << car_follower_1.x[i] << "," << car_follower_1.y[i] << "," << car_follower_2.x[i] << "," << car_follower_2.y[i] << endl;
    }
    cout << car_follower_2.x[249] << endl;
    cout << car_follower_2.y[249] << endl;
    oFile.close();
}